﻿using System;

namespace SenRobCSTest
{
    // class that calls correct functions in Ethernet to perform specific commands on the DX100 controller.
    // for more information regarding DX100 controller commands see DX100 documentation
    class Commands
    {
        // relay object to handle ethernet communication
        private Ethernet eth;
        private bool allow_rotate = true;
        public bool AllowRotation { set { this.allow_rotate = value; } get { return this.allow_rotate; } }

        // public constructor
        public Commands(Ethernet ethInstance)
        {
            eth = ethInstance;
        }

        // add preface for all commands
        private void SendCommand(String cmdName, int cmdSize)
        {
            this.eth.Send("HOSTCTRL_REQUEST " + cmdName + " " + cmdSize);
        }

        // read the alarms currently activated
        public string[] RALARM()
        {
            SendCommand("RALARM", 0);
            eth.Receive();
            return eth.ReceiveString();
        }

        // clear any current alarms
        public int RESET()
        {
            eth.print("Reset alarms");
            SendCommand("RESET", 0);
            eth.Receive(false);
            return (int)(eth.Receive()[0]);
        }

        // read the status of the robot
        // returns the read data in binary format
        public double[] RSTATS()
        {
            double[] recData;
            SendCommand("RSTATS", 0);
            try
            {
                eth.Receive(false);
                recData = eth.Receive(true);
                return recData;
            }
            catch (Exception e)
            {
                eth.printError(e);
                return null;
            }
        }

        // read currently selected robot
        public void RGROUP()
        {
            eth.print("RGROUP");
            SendCommand("RGROUP", 0);
            eth.Receive(false);
            eth.Receive(true);
        }

        // select different robot
        public void CGROUP(int data)
        {
            SendCommand("CGROUP", 4);
            this.eth.Receive();
            this.eth.SendData(data.ToString() + ",0\r");
            this.eth.Receive();
        }

        // change the current task
        public void CTASK(int data)
        {
            SendCommand("CTASK", 2);
            eth.Receive();
            eth.SendData(data.ToString() + "\r");
            eth.Receive();
        }

        // read io values. not used
        public int IOREAD()
        {
            string data = "50010,800\r";
            SendCommand("IOREAD", data.Length);
            eth.Receive(false);
            eth.SendData(data);
            eth.Receive(false);

            return 0;
        }

        // incremental movement on currently selected robot
        // returns int : 0 represents successful command
        public int IMOV(double speed, double x, double y, double z, double rx, double ry, double rz, double re)
        {
            // 3D testing
             string data;
             if(this.AllowRotation)
                 data = "0," + speed + ",0," + -y + "," + x + "," + z + "," + rx + "," + ry + "," + rz + "," + re + ",0,0,0,0,0,0,0,0\r";
             else
                 data = "0," + speed + ",0," + -y + "," + x + "," + z + "," + 0 + "," + 0 + "," + 0 + "," + 0 + ",0,0,0,0,0,0,0,0\r";

             SendCommand("IMOV", data.Length);
             eth.Receive(false);
             eth.SendData(data);
             return (int)(eth.Receive(false)[0]);

            // NORMAL TESTING
            // string data;
            // if(this.AllowRotation)
            //     data = "0," + speed + ",0," + x + "," + y + "," + z + "," + rx + "," + ry + "," + rz + "," + re + ",0,0,0,0,0,0,0,0\r";
            // else
            //     data = "0," + speed + ",0," + x + "," + y + "," + z + "," + 0 + "," + 0 + "," + 0 + "," + 0 + ",0,0,0,0,0,0,0,0\r";

            // SendCommand("IMOV", data.Length);
            // eth.Receive(false);
            // eth.SendData(data);
            // return (int)(eth.Receive(false)[0]);
        }

        // pulse movement in joint motion
        // returns int : 0 represents successful command
        public int PMOVJ(double speed, int s, int l, int u, int r, int b, int t, int e)
        {
            string data = speed + "," + s + "," + l + "," + u + "," + r + "," + b + "," + t + "," + e + ",0,0,0,0,0,0,0\r";
            SendCommand("PMOVJ", data.Length);
            eth.Receive(false);
            eth.SendData(data);
            return (int)(eth.Receive(false)[0]);
        }

        // absolute movement in joint motion
        // returns int : 0 represents successful command
        public int MOVJ(double speed, double x, double y, double z, double rx, double ry, double rz, double re)
        {
            string data = speed + ",0," + x + "," + y + "," + z + "," + rx + "," + ry + "," + rz + "," + re + ",0,4,0,0,0,0,0,0\r";
            SendCommand("MOVJ", data.Length);
            eth.Receive(false);
            eth.SendData(data);
            return (int)(eth.Receive(false)[0]);
        }

        // absolute movement in circular motion - not used
        // returns int : 0 represents successful command
        public int MOVC(double speed, double x, double y, double z, double rx, double ry, double rz, double re)
        {
            string data = speed + ",0," + x + "," + y + "," + z + "," + rx + "," + ry + "," + rz + "," + re + ",0,4,0,0,0,0,0,0\r";
            SendCommand("MOVJ", data.Length);
            eth.Receive(false);
            eth.SendData(data);
            return (int)(eth.Receive(false)[0]);
        }

        // absolute movement in linear motion
        // returns int : 0 represents successful command
        public int MOVL(double speed, double x, double y, double z, double rx, double ry, double rz, double re)
        {
            string data = "0," + speed + ",0," + x + "," + y + "," + z + "," + rx + "," + ry + "," + rz + "," + re + ",0,0,0,0,0,0,0,0\r";
            SendCommand("MOVL", data.Length);
            eth.Receive(false);
            eth.SendData(data);
            return (int)eth.Receive(false)[0];
        }

        // turn servo motors on or off
        public void SVON(bool on)
        {
            SendCommand("SVON", 2);
            eth.Receive(false);
            if (on)
                eth.SendData("1\r");
            else
                eth.SendData("0\r");
            eth.Receive(false);
        }

        // start a specific job on the robot
        // not used
        public void START()
        {
            string jobName = "Albie";
            SendCommand("START", 0); //jobName.Length + 1);
            eth.Receive();
            //eth.SendData(jobName + "\r");
            eth.Receive();
        }

        // read current job detail
        public string[] RJSEQ()
        {
            SendCommand("RJSEQ", 0);
            eth.Receive();
            return eth.ReceiveString();
        }

        // read current position in carthesian coordinate system
        public double[] RPOSC()
        {
            SendCommand("RPOSC", 4);
            eth.Receive(false);
            eth.SendData("0,1\r");
            return eth.Receive(false);
        }

        // read current position in joint coordinate system
        public double[] RPOSJ()
        {
            SendCommand("RPOSJ", 0);
            eth.Receive(false);
            return eth.Receive(false);
        }

        // toggle hold - which enables or disables movement
        public int HOLD(int onoff)
        {
            eth.print("HOLD " + onoff);
            SendCommand("HOLD", 2);
            eth.Receive(false);
            eth.SendData(onoff.ToString() + "\r");
            return (int)(eth.Receive()[0]);
        }

        // not used
        public int CYCLE(int data)
        {
            SendCommand("CYCLE", 2);
            eth.Receive();
            eth.SendData(data.ToString() + "\r");
            return (int)(eth.Receive()[0]);
        }
    }
}